{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fcf4cc05",
   "metadata": {},
   "outputs": [],
   "source": [
    "%serialconnect --port=COM15"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1af91dbd",
   "metadata": {},
   "outputs": [],
   "source": [
    "%rebootdevice"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c4be978c",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tools import CANMotorController"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3e14d2b7",
   "metadata": {},
   "outputs": [],
   "source": [
    "from esp32 import CAN\n",
    "# TX,GPIO15,PIN 21\n",
    "# RX,GPIO13,PIN 20\n",
    "bus = CAN(0, tx=15, rx=13, baudrate=1000000, mode=CAN.NORMAL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7a58ca03",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(bus)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9d484e82",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor = CANMotorController(bus, motor_id=127, main_can_id=254)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d2190e92",
   "metadata": {},
   "source": [
    "## Jog\n",
    "单个参数写入（通信类型 18）"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1c2b57c6",
   "metadata": {},
   "outputs": [],
   "source": [
    "jog_vel = 5  # rad/s\n",
    "uint_value = motor._float_to_uint(jog_vel, motor.V_MIN, motor.V_MAX, 16)\n",
    "jog_vel_bytes = motor.format_data(data=[uint_value], format=\"u16\", type=\"encode\")[:2][::-1]\n",
    "\n",
    "data1 = [0x05, 0x70, 0x00, 0x00, 0x07, 0x01] + jog_vel_bytes\n",
    "motor.clear_can_rx()\n",
    "received_msg_data, received_msg_arbitration_id = motor.send_receive_can_message(\n",
    "    cmd_mode=motor.CmdModes.SINGLE_PARAM_WRITE, data2=motor.MAIN_CAN_ID, data1=data1\n",
    ")\n",
    "motor.parse_received_msg(received_msg_data, received_msg_arbitration_id)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b977c4b0",
   "metadata": {},
   "source": [
    "## 停止"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9e6e0634",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.disable()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b53ca7d0",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.set_0_pos()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e4ef7cca",
   "metadata": {},
   "source": [
    "## 读取速度和位置\n",
    "- 发送参数：单个参数写⼊（通信类型18），写入地址0x7018（电流限制）值为 27A\n",
    "- 读取返回值：电机反馈数据（通信类型2），Byte0,1: 当前⻆度，Byte2,3:当前⻆速度"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "80df3964",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(param_name=\"limit_cur\", value=27)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fdcef3d0",
   "metadata": {},
   "source": [
    "# 位置模式\n",
    "## 发送电机模式参数写入命令（通信类型 18）\n",
    "设置 `runmode` 参数为 1\n",
    "- index(Byte0~1): `run_mode`，0x7005\n",
    "- value(Byte4~7): 1(位置模式)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9c280a67",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(\"run_mode\", value=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bca0d55c",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.enable()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "83be3d62",
   "metadata": {},
   "source": [
    "## 最大速度：发送电机模式参数写入命令（通信类型 18）\n",
    "设置 `limit_spd` 参数为预设最大速度指令\n",
    "- index(Byte0~1): `limit_spd`, 0x7017\n",
    "- value(Byte4~7): `float` [0,30]rad/s"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc433bc1",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(\"limit_spd\", value=10)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "903e4ed4",
   "metadata": {},
   "source": [
    "## 目标位置：发送电机模式参数写入命令（通信类型 18）\n",
    "设置 `loc_ref` 参数为预设位置指令\n",
    "- index(Byte0~1): `loc_ref`, 0x7016\n",
    "- value(Byte4~7): `float` rad"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a07fba09",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.write_single_param(\"loc_ref\", value=3.14)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7be81906",
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "# 定义音符到电机参数的映射，为后面的音符分配更高的转速值\n",
    "note_to_speed = {\"C\": 6.6, \"D\": 6.8, \"E\": 7.3, \"F\": 8, \"G\": 9, \"A\": 10, \"B\": 13}\n",
    "\n",
    "# 定义小星星的音符和时长\n",
    "# 完整定义小星星的音符和时长\n",
    "notes = [\"C\", \"C\", \"G\", \"G\", \"A\", \"A\", \"G\",\n",
    "         \"F\", \"F\", \"E\", \"E\", \"D\", \"D\", \"C\",\n",
    "         \"G\", \"G\", \"F\", \"F\", \"E\", \"E\", \"D\",\n",
    "         \"G\", \"G\", \"F\", \"F\", \"E\", \"E\", \"D\",\n",
    "         \"C\", \"C\", \"G\", \"G\", \"A\", \"A\", \"G\",\n",
    "         \"F\", \"F\", \"E\", \"E\", \"D\", \"D\", \"C\"]\n",
    "\n",
    "durations = [1, 1, 1, 1, 1, 1, 2,\n",
    "             1, 1, 1, 1, 1, 1, 2,\n",
    "             1, 1, 1, 1, 1, 1, 2,\n",
    "             1, 1, 1, 1, 1, 1, 2,\n",
    "             1, 1, 1, 1, 1, 1, 2,\n",
    "             1, 1, 1, 1, 1, 1, 2]  # 以四分音符为单位\n",
    "\n",
    "x_speed = 1.5 # 速度倍数\n",
    "x_duration = 0.4 # 间隔倍数，如果小于1，就是减小时间间隔\n",
    "\n",
    "\n",
    "# 初始化电机的当前位置和方向标志\n",
    "current_loc_ref = 0\n",
    "direction_flag = 1  # 1表示正方向，-1表示反方向\n",
    "\n",
    "motor.disable() #\n",
    "motor.set_motor_position_control(limit_spd=10, loc_ref=0)\n",
    "motor.set_0_pos()  # 当前0位\n",
    "motor.enable()\n",
    "\n",
    "# 播放小星星\n",
    "for note, duration in zip(notes, durations):\n",
    "    duration = duration * x_duration\n",
    "    \n",
    "    # 计算电机的转速和目标位置\n",
    "    limit_spd = note_to_speed[note] * x_speed\n",
    "    loc_ref = current_loc_ref + (direction_flag * limit_spd * duration * 1.5)\n",
    "\n",
    "    # 使用电机参数设置函数\n",
    "    motor.set_motor_position_control(limit_spd=limit_spd, loc_ref=loc_ref)\n",
    "\n",
    "    # 更新电机的当前位置\n",
    "    current_loc_ref = loc_ref\n",
    "\n",
    "    # 反转方向标志\n",
    "#     direction_flag *= -1\n",
    "\n",
    "    # 休眠一段时间以模拟音符的时长（这里假设一个四分音符的时长是1秒）\n",
    "    time.sleep(duration * x_speed)\n",
    "motor.disable()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "98831b4d",
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.disable()\n",
    "motor.set_0_pos()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a00ed80c",
   "metadata": {},
   "outputs": [],
   "source": [
    "%rebootdevice"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e2293782",
   "metadata": {},
   "outputs": [],
   "source": [
    "%disconnect"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5778b252",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "af29cc65",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "MicroPython - USB",
   "language": "micropython",
   "name": "micropython"
  },
  "language_info": {
   "codemirror_mode": "python",
   "file_extension": ".py",
   "mimetype": "text/python",
   "name": "micropython"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
